package snucse.teamd;

import java.util.HashMap;
import java.util.Iterator;
import java.util.Vector;

import snucse.teamd.math.Vec2;

public class World {
	private Vec2 gravity;
	private Vector<Body> bodies;
	//private HashMap<ArbiterKey, Arbiter> arbiters;//arbiter between two bodies
	public Vector<Arbiter> arbiterList = new Vector<Arbiter>();
	private int iterations;//will apply force how many times
	private boolean isPlaying;
	public World(){
		setPlaying(true);
		gravity=new Vec2(0, -10f);
		bodies=new Vector<Body>();
		//arbiters=new HashMap<ArbiterKey, Arbiter>();
		iterations=20;		
	}
	
	public void reset() {
		setPlaying(true);
		gravity=new Vec2(0, -10f);
		bodies=new Vector<Body>();
		//arbiters=new HashMap<ArbiterKey, Arbiter>();
		iterations=20;
	}
	
	
	public Vector<Body> getBodies() {
		return bodies;
	}

	public void setBodies(Vector<Body> bodies) {
		this.bodies = bodies;
	}
	public void addBody(Body body){
		bodies.addElement(body);
	}

	//public HashMap<ArbiterKey, Arbiter> getArbiters() {
	//	return arbiters;
	//}

	//public void setArbiters(HashMap<ArbiterKey, Arbiter> arbiters) {
	//	this.arbiters = arbiters;
	//}

	public int getIterations() {
		return iterations;
	}

	public void setIterations(int iterations) {
		this.iterations = iterations;
	}

	public void setGravityX(double x) {
		gravity.setX(x);
	}

	public void setGravityY(double y) {
		gravity.setY(y);
	}
	public boolean isExist(Arbiter arb, float dt){
		boolean isTrue=false;
		Arbiter existArb;
		//System.out.println("	 "+arbiterList.size());
		for (int i=0; i< arbiterList.size(); i++){
			existArb=arbiterList.get(i);
			/*if (existArb.numOfContacts!=arb.numOfContacts) continue;
			for (int j=0; j<existArb.numOfContacts;j++){
				if (Math.abs(Vec2.distance(existArb.getContacts().get(j).getPosition(), 
						arb.getContacts().get(j).getPosition())) < 0.1f){
					isTrue=true;
				}
			}*/
			existArb.timeout+=dt;
			if ((existArb.b1.equals(arb.b1) &&existArb.b2.equals(arb.b2))||(existArb.b1.equals(arb.b2) &&existArb.b2.equals(arb.b1))) {
				return true;
			}
		}
		return false;
	}
	private void update(float dt) {
		int j;
		Body b1, b2;
		SubTriangle t1, t2;
		Vector<Vec2> vertices;
		Vec2 dCOM;
		//arbiterList=new Vector<Arbiter>();
		// Determine overlapping bodies and update contact points.
		for (int i = 0; i < (int)bodies.size(); i++)
		{
			b1 = bodies.get(i);
			for (j = i+1; j<bodies.size();j++){
				b2 = bodies.get(j);
				//detect collision.
				Arbiter arbiter = new Arbiter(b1,b2);
				if (Collide.detectCollision(b1, b2, arbiter)){
					//dt=0;
					//if b1 collides with b2, Arbiter and ArbiterKey should be constructed
			//		ArbiterKey key = new ArbiterKey(b1,b2);
			//		arbiters.put(key, arbiter);
					//System.out.println("collision");
					if (!isExist(arbiter, dt)){
						//System.out.println("collision "+arbiterList.size());
						arbiterList.addElement(arbiter);
					}
				}
			}
		}
		for (int i=0; i< arbiterList.size();i++){
			arbiterList.get(i).timeout+=dt;
			if (arbiterList.get(i).timeout>20f/60f) {
				arbiterList.remove(i);
				i--;
			}
		}

		// Integrate forces.(actually just gravity for velocity)
		for (int i = 0; i < (int)bodies.size(); ++i)
		{
			Body b = bodies.get(i);

			if(b.isEnv) continue;
			b.setTorque( b.friction *b.mass * b.radius) ;
			b.setVel(
				Vec2.plus(
					b.getVel(), 
					Vec2.scalar(
						Vec2.plus(
							gravity , 
							Vec2.scalar(b.force, 1/b.getMass())), 
						dt
					)));
			if(b.getAngularVelocity() > 0)
			{
				b.setAngularVelocity(
						b.getAngularVelocity() - (b.torque/b.I) * dt
						);
			}
			
			else{
				b.setAngularVelocity(
						b.getAngularVelocity() + (b.torque/b.I) * dt
						);
			}
			
			//consider friction
			//if body has contact point on Env, care about friction
			
		}

		// calculate values and impulse J at contact point
		//for ( Iterator<ArbiterKey> I = arbiters.keySet().iterator(); I.hasNext(); I.next())
		for (int i=0; i<arbiterList.size();i++)
		{
			Arbiter arbiter = arbiterList.get(i);//arbiters.get(I.next());
			arbiter.ApplyImpulse();
			Body temp1 = arbiter.b1;
			Body temp2 = arbiter.b2;
			//System.out.println(i +" th iter) body1 - AngVel :"+ temp1.getAngularVelocity()+" Vel :"+ temp1.getVel());
			//System.out.println(i +" th iter) body2 - AngVel :"+ temp2.getAngularVelocity()+" Vel :"+ temp2.getVel());
		}
		
		// calculate impulse J by iteration
		/*
		for (int i = 0; i < iterations; ++i)
		{
			//for ( Iterator<ArbiterKey> I = arbiters.keySet().iterator(); I.hasNext(); I.next())
			for (int k=0; k<arbiterList.size();k++)
			{
				Arbiter arbiter = arbiterList.get(k);//arbiters.get(I.next());
				arbiter.ApplyImpulse();
				
			}
		}
		 */
		// moving position and rotation by velocity and angular velocity
		for (int i = 0; i < (int)bodies.size(); i++)
		{
			b1=bodies.get(i);
			if(b1.isEnv) {
				
				continue;
			}
			dCOM = new Vec2(b1.getVel().getX()*dt, b1.getVel().getY()*dt);
			b1.setRotAngle(b1.getRotAngle()+b1.getAngularVelocity()*dt);
			b1.setCenterOfMass(Vec2.plus(b1.getCenterOfMass(), dCOM));
			
			vertices = new Vector<Vec2>();
			for (j=0; j<b1.vertices.size(); j++){//each point
				vertices.addElement(Vec2.plus(
						b1.vertices.get(j), dCOM
						));	
			}
			b1.vertices=vertices;

		
			for (int k=0;k<b1.subTriangles.size();k++){
				vertices = new Vector<Vec2>();
				t1 = b1.subTriangles.get(k);
				t1.com=Vec2.plus(t1.com, dCOM);
				for(j=0; j<t1.tvertices.size();j++){
					vertices.addElement(Vec2.plus(
							t1.tvertices.get(j), dCOM
							));
				}
				t1.tvertices=vertices;
			}
		}
	}
	void Step(float dt)
	{
		if(isPlaying) update(dt);
		/*
		for(int i=0; i<bodies.size();i++){
		System.out.println(      bodies.get(i).vertices.toString());
		}*/
	}

	public Vec2 getGravity() {
		return gravity;
	}

	public void setGravity(Vec2 gravity) {
		this.gravity = gravity;
	}


	public boolean isPlaying() {
		return isPlaying;
	}


	public void setPlaying(boolean isPlaying) {
		this.isPlaying = isPlaying;
	}

}
